/*********************************************************************************************************************
 *
 * @file            softiic
 * @author          �ֹ���
 * @version         v1.0
 * @Software          ADS
 * @Target core     TC264D
 * @date            2021-03-26
 ********************************************************************************************************************/
#include "zf_common_headfile.h"
#include "IfxScuWdt.h"
#include "IfxFlash.h"
//#include "zf_assert.h"
//#include "zf_eeprom.h"
#include "zf_driver_flash.h"
//ʹ��ҳ��0
typedef enum //ö�ٱ�����ַ
{
    DEBUG_SERIALTX_ADDR, BALANCE_TEST_ADDR,

    BALANCE_VER_ADDR, LQR_q1_ADDR, LQR_q2_ADDR, LQR_q3_ADDR,

    BLACK_ADDR, WHITE_ADDR, EXP_TIME_ADDR,

    SPEED_AIM_ADDR, SPEED_P_ADDR, SPEED_I_ADDR, SPEED_D_ADDR,

    SERVO_P_ADDR, SERVO_D_ADDR, SERVORATE_D_ADDR,

    Diff_P_ADDR, Diff_D_ADDR, DIFFrate_D_ADDR,

    GAIN_ADDR, Balance_Mode_ADDR,

    Ramp_Angle_ADDR, Big_Ramp_Time_ADDR, Small_Ramp_Time_ADDR,

    One_ADDR, Two_ADDR, Three_ADDR, Four_ADDR, Five_ADDR, Six_ADDR,

    Ring_Value_ADDR, Distance_ADDR,

    Rone_ADDR, Rtwo_ADDR, Rthree_ADDR,

    Mod_Data_ADDR,

    PID_roll_velocity_q1_ADDR, PID_roll_velocity_q2_ADDR, PID_roll_velocity_q3_ADDR,

    PID_roll_q1_ADDR, PID_roll_q2_ADDR, PID_roll_q3_ADDR,

    PID_flywheel_q1_ADDR, PID_flywheel_q2_ADDR, PID_flywheel_q3_ADDR,
    zero_p_r_ADDR, zero_p_l_ADDR,

    PID_turn_speed_q1_ADDR, PID_turn_speed_q2_ADDR, PID_turn_speed_q3_ADDR,

    LQR_zero_p_ADDR, LQR_zero_d_ADDR,
    LQR_zero_r_ADDR, LQR_zero_l_ADDR,
    LQR_zero_max_ADDR,

    SERVO_I_ADDR,
    LQR_zero_i_ADDR,

    ring_straight_ADDR, ring_close_ADDR, ring_en_ADDR,
    ring_4_ADDR, ring_5_ADDR, ring_6_ADDR,

    garage_en_out_ADDR, garage_en_in_ADDR, garage_3_ADDR,
    garage_4_ADDR, garage_5_ADDR, garage_6_ADDR,

    forkroad_en_ADDR, forkroad_left_right_ADDR,
} Debug_Addr_enum;

//ʹ��ҳ��3
typedef enum //ö�ٱ�����ַ
{
    PID_ZL_jiaosudu_Kp_ADDR,PID_ZL_jiaosudu_Ki_ADDR,
    PID_ZL_jiaodu_Kp_ADDR,PID_ZL_jiaodu_Kd_ADDR,
    PID_ZL_Speed_Kp_ADDR,PID_ZL_Speed_Ki_ADDR,PID_ZL_Speed_Kd_ADDR,
    TURN_Diff_P_ADDR,TURN_Diff_D_ADDR,
    AngleAim_Buf_ADDR,Speed_Aim_Buf_ADDR,
    Exp_Time_Buf_ADDR,Gain_Buf_ADDR,
}Debug_Addr_enum1;
/*****�洢��������*****/
int32 Balance_VER_Buf = 0;
int32 black_Buf = 75, white_Buf = 150;
int32 PID_turn_speed_q1_Buf = 0, PID_turn_speed_q2_Buf = 0, PID_turn_speed_q3_Buf = 0;
int32 Servo_P_Buf = 0, Servo_D_Buf = 0, Servo_I_Buf = 0, Servorate_D_Buf = 0;
int32 LQR_zero_p_Buf, LQR_zero_i_Buf, LQR_zero_d_Buf, LQR_zero_r_Buf, LQR_zero_l_Buf, LQR_zero_max_Buf;
int32 LQR_q1_Buf = 0, LQR_q2_Buf = 0, LQR_q3_Buf = 0;

//int32 Exp_Time_Buf = 0, Gain_Buf = 0;
int32 Balance_Mode_Buf = 0;
int32 Ramp_Angle_Buf = 0, Big_Ramp_Time_Buf = 0, Small_Ramp_Time_Buf = 0;
int32 One_Buf = 0, Two_Buf = 0, Three_Buf = 0, Four_Buf = 0, Five_Buf = 0, Six_Buf = 0;
int32 Rone_Buf = 0, Rtwo_Buf = 0, Rthree_Buf = 0;
int32 Ring_Value_Buf = 0;
int32 Distance_Buf = 0;

int32 PID_roll_velocity_q1_Buf = 0, PID_roll_velocity_q2_Buf = 0, PID_roll_velocity_q3_Buf = 0;
int32 PID_roll_q1_Buf = 0, PID_roll_q2_Buf = 0, PID_roll_q3_Buf = 0;
int32 PID_flywheel_q1_Buf = 0, PID_flywheel_q2_Buf = 0, PID_flywheel_q3_Buf = 0;

int32 ring_straight_Buf = 0;
int32 ring_close_Buf = 0;
int32 ring_en_Buf = 0;

int32 garage_en_out_Buf = 0;
int32 garage_en_in_Buf = 0;

int32 forkroad_en_Buf = 0;
int32 forkroad_left_right_Buf = 0;

//ֱ������
//
int32 PID_ZL_jiaosudu_Kp_Buf = 0,PID_ZL_jiaosudu_Ki_Buf = 0;//57 10
int32 PID_ZL_jiaodu_Kp_Buf = 0,PID_ZL_jiaodu_Kd_Buf = 0;//-444
int32 PID_ZL_Speed_Kp_Buf = 0,PID_ZL_Speed_Ki_Buf = 0,PID_ZL_Speed_Kd_Buf = 0;
int32 TURN_Diff_P_Buf = 0,TURN_Diff_D_Buf = 0;
int32 AngleAim_Buf = 0,Speed_Aim_Buf = 0;//�Ƕ��������ٶ�����
int32 Balance_Test = 0; //0��������ʾͼ��ƫ���������1��ʾ���������

/* @blue_test��
 * 0 ��������
 * 1 �ٶȻ����ߵ���ģʽ,ֹͣת����� */
uint8 blue_test = 0;

int32 Speed_P_Buf = 0, Speed_I_Buf, Speed_D_Buf = 0;
int32 Exp_Time_Buf = 0,Gain_Buf = 0;
/*
 *  图传标志位
 *  进行图像的传输
 * */
uint8 TC_flag = 0;

/*****ʵ�ʲ�������*****/
float Balance_VER = 0;
float Speed_Aim = 0;
float Speed_P = 0, Speed_I = 0, Speed_D = 0;
float Servo_P = 0, Servo_D = 0, Servo_I, Servorate_D = 0;
float Diff_P = 0, Diff_D = 0, DIFFrate_D = 0;
uint8 Balance_Mode = 0;
uint8 one = 25, two = 0, three = 0, four = 0, five = 0, six = 0;
uint8 Rone = 0, Rtwo = 0, Rthree = 0;

int Distance = 0;
/******���Բ���******/


uint8 Debug_SerialTX = 3;
uint8 menu_en = 1;

float tureKp = 0,tureKd = 0;
void User_StartMenu (void)
{
    int Key_Data = 0, Key_Time = 0, Lis_Data = 1, Mod_Data = 0;

    menu_en = 1;
    system_delay_ms(10);

    /*****flash�����洢����*****/
    system_delay_ms(10);
    UDF_flash_page_read(ZL_EEPROM_SECTOR, (uint32*) &PID_ZL_jiaosudu_Kp_Buf, PID_ZL_jiaosudu_Kp_ADDR);
    UDF_flash_page_read(ZL_EEPROM_SECTOR, (uint32*) &PID_ZL_jiaosudu_Ki_Buf, PID_ZL_jiaosudu_Ki_ADDR);
    system_delay_ms(10);
    UDF_flash_page_read(ZL_EEPROM_SECTOR, (uint32*) &PID_ZL_jiaodu_Kp_Buf, PID_ZL_jiaodu_Kp_ADDR);
    UDF_flash_page_read(ZL_EEPROM_SECTOR, (uint32*) &PID_ZL_jiaodu_Kd_Buf, PID_ZL_jiaodu_Kd_ADDR);
    system_delay_ms(10);
    UDF_flash_page_read(ZL_EEPROM_SECTOR, (uint32*) &PID_ZL_Speed_Kp_Buf, PID_ZL_Speed_Kp_ADDR);
    UDF_flash_page_read(ZL_EEPROM_SECTOR, (uint32*) &PID_ZL_Speed_Ki_Buf, PID_ZL_Speed_Ki_ADDR);
    UDF_flash_page_read(ZL_EEPROM_SECTOR, (uint32*) &PID_ZL_Speed_Kd_Buf, PID_ZL_Speed_Kd_ADDR);
    system_delay_ms(10);
    UDF_flash_page_read(ZL_EEPROM_SECTOR, (uint32*) &TURN_Diff_P_Buf, TURN_Diff_P_ADDR);
    UDF_flash_page_read(ZL_EEPROM_SECTOR, (uint32*) &TURN_Diff_D_Buf, TURN_Diff_D_ADDR);
    system_delay_ms(10);
    UDF_flash_page_read(ZL_EEPROM_SECTOR, (uint32*) &AngleAim_Buf, AngleAim_Buf_ADDR);
    UDF_flash_page_read(ZL_EEPROM_SECTOR, (uint32*) &Speed_Aim_Buf, Speed_Aim_Buf_ADDR);
    system_delay_ms(10);
    UDF_flash_page_read(ZL_EEPROM_SECTOR, (uint32*) &Exp_Time_Buf, Exp_Time_Buf_ADDR);
    UDF_flash_page_read(ZL_EEPROM_SECTOR, (uint32*) &Gain_Buf, Gain_Buf_ADDR);
//    Speed_Aim_Buf = 0;
//    TURN_Diff_P_Buf = 0;
    /*****flash�����洢����*****/
    while (Key_Data != 5)
    {
        Key_Data = 0;
        Key_Time = 0;

//        while (Key_Data == 0 && (fabs(Encoder_LeftSpeed) < 0.05) && menu_en == 1)
        while (Key_Data == 0  && menu_en == 1)
        {
            Key_Data = Key_Read();                    //�ȴ�����
        }

        while (Key_Read() == 2 && (menu_en == 1))
        {
            //�˳��б����2���ð����˳�
            if (Key_Data == 2)
            {
                system_delay_ms(50);
                Key_Time++;
            }
            if (Key_Time > 5)
            {
                Key_Data = 5;
                break;
            }
        }

        /***********ѡ����***********/
        if (Key_Data == 1 && menu_en == 1)
            switch (Lis_Data)
            {
                case 1 :
                    oled_show_string(0, 2, "  ");
                    oled_show_string(0, 4, "->");
                    oled_show_string(0, 6, "  ");
                    Lis_Data = 2;
                    encoder_add_flag = 0;
                    break;
                case 2 :
                    oled_show_string(0, 2, "  ");
                    oled_show_string(0, 4, "  ");
                    oled_show_string(0, 6, "->");
                    Lis_Data = 3;
                    encoder_add_flag = 0;
                    break;
                case 3 :
                    oled_show_string(0, 2, "->");
                    oled_show_string(0, 4, "  ");
                    oled_show_string(0, 6, "  ");
                    Lis_Data = 1;
                    encoder_add_flag = 0;
                    break;
                default :
                    break;
            }
        /***********����ҳ***********/
        if (Key_Data == 2 && menu_en == 1)
            switch (Mod_Data)
            {
                case 0 :
                    oled_clear();
                    oled_show_string(7, 0, "    DeBug");
                    oled_show_int(80, 2, Balance_Test, 3);
                    oled_show_string(0, 2, "->MS:");
                    oled_show_int(80, 4, blue_test, 3);
                    oled_show_string(0, 4, "GN:");
                    oled_show_int(80, 6, TC_flag, 3);
                    oled_show_string(0, 6, "TC:");

                    Mod_Data = 1;
                    Lis_Data = 1;
                    break;
                case 1 :
                    oled_clear();
                    oled_show_string(7, 0, "    SeepPID");
                    oled_show_int(80, 2, PID_ZL_Speed_Kp_Buf, 3);
                    oled_show_string(0, 2, "->Kp:");
                    oled_show_int(80, 4, PID_ZL_Speed_Ki_Buf, 3);
                    oled_show_string(0, 4, "   Ki:");
                    oled_show_int(80, 6, PID_ZL_Speed_Kd_Buf, 3);
                    oled_show_string(0, 6, "   Kd:");
                    Mod_Data = 2;
                    Lis_Data = 1;
                    break;
                case 2 :
                    oled_clear();
                    oled_show_string(7, 0, "    jiaosudu");
                    oled_show_int(80, 2, PID_ZL_jiaosudu_Kp_Buf, 3);
                    oled_show_string(0, 2, "->Kp:");
                    oled_show_int(80, 6, PID_ZL_jiaosudu_Ki_Buf, 3);
                    oled_show_string(0, 6, "   Ki:");
                    Mod_Data = 3;
                    Lis_Data = 1;
                    break;
                case 3 :
                    oled_clear();
                    oled_show_string(7, 0, "    jiaodu");
                    oled_show_int(80, 2, PID_ZL_jiaodu_Kp_Buf, 3);
                    oled_show_string(0, 2, "->Kp:");
                    oled_show_int(80, 6, PID_ZL_jiaodu_Kd_Buf, 3);
                    oled_show_string(0, 6, "   Kd:");
                    Mod_Data = 4;
                    Lis_Data = 1;
                    break;
                case 4 :
                    oled_clear();
                    oled_show_string(7, 0, "    Turn_C");
                    oled_show_int(80, 2, TURN_Diff_P_Buf, 4);
                    oled_show_string(0, 2, "->Kp:");
                    oled_show_int(80, 6, TURN_Diff_D_Buf, 4);
                    oled_show_string(0, 6, "   Kd:");
                    Mod_Data = 5;
                    Lis_Data = 1;
                    break;
                case 5 :
                    oled_clear();
                    oled_show_string(7, 0, "    QiWang");
                    oled_show_int(80, 2, AngleAim_Buf, 4);
                    oled_show_string(0, 2, "->An:");
                    oled_show_int(80, 6, Speed_Aim_Buf, 4);
                    oled_show_string(0, 6, "   Sp:");
                    Mod_Data = 6;
                    Lis_Data = 1;
                    break;
                case 6 :
                    oled_clear();
                    oled_show_string(7, 0, "tux_ZenYu");
                    oled_show_int(80, 2, Exp_Time_Buf, 4);
                    oled_show_string(0, 2, "->Exp:");
                    oled_show_int(80, 6, Gain_Buf, 4);
                    oled_show_string(0, 6, "  Gain:");
                    Mod_Data = 0;
                    Lis_Data = 1;
                    break;
                default :
                    break;
            }
        /***********�Ӳ���***********/
        if (Key_Data == 3 && menu_en == 1)
        {
            switch (Mod_Data)
            {
                case 1:
                    switch (Lis_Data)
                    {
                        case 1 :
                            Balance_Test += 1;
                            oled_show_int(80, 2, Balance_Test, 3);
                            oled_show_string(0, 2, "->MS: ");
                            break;
                        case 2 :
                            blue_test += 1;
                            oled_show_int(80, 4, blue_test, 3);
                            oled_show_string(0, 4, "->GN:");
                            break;
                        case 3 :
                            TC_flag += 1;
                            oled_show_int(80, 6, TC_flag, 3);
                            oled_show_string(0, 6, "->TC:");
                            break;
                        default :
                            break;
                    }
                    break;
                case 2 :
                    switch (Lis_Data)
                    {
                        case 1 :
                            PID_ZL_Speed_Kp_Buf += 1;
                            oled_show_int(80, 2, PID_ZL_Speed_Kp_Buf, 4);
                            oled_show_string(0, 2, "->Kp: ");
                            break;
                        case 2 :
                            PID_ZL_Speed_Ki_Buf += 1;
                            oled_show_int(80, 4, PID_ZL_Speed_Ki_Buf, 4);
                            oled_show_string(0, 4, "->Ki: ");
                            break;
                        case 3 :
                            PID_ZL_Speed_Kd_Buf += 1;
                            oled_show_int(80, 6, PID_ZL_Speed_Kd_Buf, 4);
                            oled_show_string(0, 6, "->Kd: ");
                            break;
                        default :
                            break;
                    }
                    break;
                case 3 :
                    switch (Lis_Data)
                    {
                        case 1 :
                            PID_ZL_jiaosudu_Kp_Buf += 1;
                            oled_show_int(80, 2, PID_ZL_jiaosudu_Kp_Buf, 4);
                            oled_show_string(0, 2, "->Kp: ");
                            break;
                        case 3 :
                            PID_ZL_jiaosudu_Ki_Buf += 1;
                            oled_show_int(80, 6, PID_ZL_jiaosudu_Ki_Buf, 4);
                            oled_show_string(0, 6, "->Ki: ");
                            break;
                        default :
                            break;
                    }
                    break;
                case 4 :
                    switch (Lis_Data)
                    {
                        case 1 :
                            PID_ZL_jiaodu_Kp_Buf += 1;
                            oled_show_int(80, 2, PID_ZL_jiaodu_Kp_Buf, 4);
                            oled_show_string(0, 2, "->Kp: ");
                            break;
                        case 3 :
                            PID_ZL_jiaodu_Kd_Buf += 1;
                            oled_show_int(80, 6, PID_ZL_jiaodu_Kd_Buf, 4);
                            oled_show_string(0, 6, "->Kd: ");
                            break;
                        default :
                            break;
                    }
                    break;
                case 5 :
                    switch (Lis_Data)
                    {
                        case 1 :
                            TURN_Diff_P_Buf += 1;
                            oled_show_int(80, 2, TURN_Diff_P_Buf, 4);
                            oled_show_string(0, 2, "->Kp: ");
                            break;
                        case 3 :
                            TURN_Diff_D_Buf += 1;
                            oled_show_int(80, 6, TURN_Diff_D_Buf, 4);
                            oled_show_string(0, 6, "->Kd: ");
                            break;
                        default :
                            break;
                    }
                    break;
                case 6 :
                    switch (Lis_Data)
                    {
                        case 1 :
                            AngleAim_Buf += 1;
                            oled_show_int(80, 2, AngleAim_Buf, 4);
                            oled_show_string(0, 2, "->An: ");
                            break;
                        case 3 :
                            Speed_Aim_Buf += 1;
                            oled_show_int(80, 6, Speed_Aim_Buf, 4);
                            oled_show_string(0, 6, "->Sp: ");
                            break;
                        default :
                            break;
                    }
                    break;
                case 0 :
                    switch (Lis_Data)
                    {
                        case 1 :
                            Exp_Time_Buf += 1;
                            oled_show_int(80, 2, Exp_Time_Buf, 4);
                            oled_show_string(0, 2, "->Exp:");
                            break;
                        case 3 :
                            Gain_Buf += 1;
                            oled_show_int(80, 6, Gain_Buf, 4);
                            oled_show_string(0, 6, "->Gain:");
                            break;
                        default :
                            break;
                    }
                    break;
                default :
                    break;
            }
        }
        /***********������***********/
        if (Key_Data == 4 && menu_en == 1)
        {
            switch (Mod_Data)
            {
                case 1:
                    switch (Lis_Data)
                    {
                        case 1 :
                            Balance_Test -= 1;
                            oled_show_int(80, 2, Balance_Test, 3);
                            oled_show_string(0, 2, "->MS: ");
                            break;
                        case 2 :
                            blue_test -= 1;
                            oled_show_int(80, 4, blue_test, 3);
                            oled_show_string(0, 4, "->GN:");
                            break;
                        case 3 :
                            TC_flag = 1;
                            oled_show_int(80, 6, TC_flag, 3);
                            oled_show_string(0, 6, "->TC:");
                            break;
                        default :
                            break;
                    }
                    break;
                case 2 :
                    switch (Lis_Data)
                    {
                        case 1 :
                            PID_ZL_Speed_Kp_Buf -= 1;
                            oled_show_int(80, 2, PID_ZL_Speed_Kp_Buf, 4);
                            oled_show_string(0, 2, "->Kp: ");
                            break;
                        case 2 :
                            PID_ZL_Speed_Ki_Buf -= 1;
                            oled_show_int(80, 4, PID_ZL_Speed_Ki_Buf, 4);
                            oled_show_string(0, 4, "->Ki: ");
                            break;
                        case 3 :
                            PID_ZL_Speed_Kd_Buf -= 1;
                            oled_show_int(80, 6, PID_ZL_Speed_Kd_Buf, 4);
                            oled_show_string(0, 6, "->Kd: ");
                            break;
                        default :
                            break;
                    }
                    break;
                case 3 :
                    switch (Lis_Data)
                    {
                        case 1 :
                            PID_ZL_jiaosudu_Kp_Buf -= 1;
                            oled_show_int(80, 2, PID_ZL_jiaosudu_Kp_Buf, 4);
                            oled_show_string(0, 2, "->Kp: ");
                            break;
                        case 3 :
                            PID_ZL_jiaosudu_Ki_Buf -= 1;
                            oled_show_int(80, 6, PID_ZL_jiaosudu_Ki_Buf, 4);
                            oled_show_string(0, 6, "->Ki: ");
                            break;
                        default :
                            break;
                    }
                    break;
                case 4 :
                    switch (Lis_Data)
                    {
                        case 1 :
                            PID_ZL_jiaodu_Kp_Buf -= 1;
                            oled_show_int(80, 2, PID_ZL_jiaodu_Kp_Buf, 4);
                            oled_show_string(0, 2, "->Kp: ");
                            break;
                        case 3 :
                            PID_ZL_jiaodu_Kd_Buf -= 1;
                            oled_show_int(80, 6, PID_ZL_jiaodu_Kd_Buf, 4);
                            oled_show_string(0, 6, "->Kd: ");
                            break;
                        default :
                            break;
                    }
                    break;
                case 5 :
                    switch (Lis_Data)
                    {
                        case 1 :
                            TURN_Diff_P_Buf -= 1;
                            oled_show_int(80, 2, TURN_Diff_P_Buf, 4);
                            oled_show_string(0, 2, "->Kp: ");
                            break;
                        case 3 :
                            TURN_Diff_D_Buf -= 1;
                            oled_show_int(80, 6, TURN_Diff_D_Buf, 4);
                            oled_show_string(0, 6, "->Kd: ");
                            break;
                        default :
                            break;
                    }
                    break;
                case 6 :
                    switch (Lis_Data)
                    {
                        case 1 :
                            AngleAim_Buf -= 1;
                            oled_show_int(80, 2, AngleAim_Buf, 4);
                            oled_show_string(0, 2, "->Kp: ");
                            break;
                        case 3 :
                            Speed_Aim_Buf -= 1;
                            oled_show_int(80, 6, Speed_Aim_Buf, 4);
                            oled_show_string(0, 6, "->S: ");
                            break;
                        default :
                            break;
                    }
                    break;
                case 0 :
                    switch (Lis_Data)
                    {
                        case 1 :
                            Exp_Time_Buf -= 1;
                            oled_show_int(80, 2, Exp_Time_Buf, 4);
                            oled_show_string(0, 2, "->Exp:");
                            break;
                        case 3 :
                            Gain_Buf -= 1;
                            oled_show_int(80, 6, Gain_Buf, 4);
                            oled_show_string(0, 6, "->Gain:");
                            break;
                        default :
                            break;
                    }
                    break;
                default :
                    break;
            }
        }
    }
//    PID_ZL_Speed_Kp_Buf = 0;
//    PID_ZL_Speed_Ki_Buf = 0;
//    PID_ZL_Speed_Kd_Buf = 0;
    ZL_sudu.q1 = (float) PID_ZL_Speed_Kp_Buf / 1000;
    ZL_sudu.q2 = (float) PID_ZL_Speed_Ki_Buf / 1000;
    ZL_sudu.q3 = (float) PID_ZL_Speed_Kd_Buf / 1000;
//    PID_ZL_jiaodu_Kd_Buf = 0;
//    Balance_P = (float) PID_ZL_jiaodu_Kp_Buf / 10;
//    Balance_D = (float) PID_ZL_jiaodu_Kd_Buf / 10;

    Speed_P = (float) PID_ZL_Speed_Kp_Buf / 1;
    Speed_I = (float) PID_ZL_Speed_Ki_Buf / 1000;
    Speed_D = (float) PID_ZL_Speed_Kd_Buf / 1000;
    Servo_P = (float) PID_ZL_jiaodu_Kp_Buf / 100;
    Servo_D = (float) PID_ZL_jiaodu_Kd_Buf / 100;
//    PID_ZL_jiaodu_Kp_Buf = 0;
//    PID_ZL_jiaodu_Kd_Buf = 0;
//    PID_ZL_jiaodu_Kd_Buf =0;
//    PID_ZL_jiaodu_Kp_Buf = 0;
//    ZL_jiaodu.q1 = (float) PID_ZL_jiaodu_Kp_Buf / 10;
//    ZL_jiaodu.q3 = (float) PID_ZL_jiaodu_Kd_Buf / 10;
//    ZL_jiaosudu.q1 = (float) PID_ZL_jiaosudu_Kp_Buf / 100;
//    ZL_jiaosudu.q3 = (float) PID_ZL_jiaosudu_Ki_Buf / 100;

    tureKp = TURN_Diff_P_Buf/100.0;
    tureKd = TURN_Diff_D_Buf/100.0;


//    Diff_P = 10;
    Speed_Aim = Speed_Aim_Buf/1.0;
    AngleAim = AngleAim_Buf/1.0;


    // ͼ������
    Exp_Time = Exp_Time_Buf;//������ʱ���������������ʱ�������
    Gain = Gain_Buf;//
//    Distance = (int) Distance_Buf;
//    Balance_Mode = (uint8) Balance_Mode_Buf;

//    Balance_VER = (float) Balance_VER_Buf / 10.0;
//
//    black = (int) black_Buf;
//    white = (int) white_Buf;
//    Speed_Aim = (float) Speed_Aim_Buf;
////
//    PID_turn_speed.q1 = (float) PID_turn_speed_q1_Buf ;
//    PID_turn_speed.q2 = (float) PID_turn_speed_q2_Buf / 10.0;
//    PID_turn_speed.q3 = (float) PID_turn_speed_q3_Buf;
//
//    Servo_P = (float) Servo_P_Buf ;
//    Servo_I = (float) Servo_I_Buf/10;
//    Servo_D = (float) Servo_D_Buf/10;
//
//    zero.r = (float) LQR_zero_r_Buf/10.0;
//    zero.l = (float) LQR_zero_l_Buf;
////
//    zero.max = (float) LQR_zero_max_Buf;

//
//    zero.p = (float) (zero.max / 40) + LQR_zero_p_Buf/10000.0;
////    LQR.zero_p = (float) LQR_zero_p_Buf/10000.0;
//    zero.i = (float) LQR_zero_i_Buf/1000000.0;
//    zero.d = (float) LQR_zero_d_Buf/100.0;
//

//    LQR.q1 = (float) LQR_q1_Buf;
//    LQR.q2 = (float) LQR_q2_Buf/10;
//    LQR.q3 = (float) LQR_q3_Buf;
//
//    Speed_P = (float) Speed_P_Buf;
//    Speed_I = (float) Speed_I_Buf / 1000.0;
//    Speed_D = (float) Speed_D_Buf;
//
//    Exp_Time = (uint8) Exp_Time_Buf;
//    Gain = (uint8) Gain_Buf;
//    ring_value = (int)Ring_Value_Buf;

//    PID_roll_velocity.q1 = (float)PID_roll_velocity_q1_Buf;
//    PID_roll_velocity.q2 = (float)PID_roll_velocity_q2_Buf / 100;
//    PID_roll_velocity.q3 = (float)PID_roll_velocity_q3_Buf;
//
//    PID_roll.q1 = (float)PID_roll_q1_Buf;
//    PID_roll.q2 = (float)PID_roll_q2_Buf / 100;
//    PID_roll.q3 = (float)PID_roll_q3_Buf;
//
//    PID_flywheel.q1 = (float)PID_flywheel_q1_Buf / 10000;
//    PID_flywheel.q2 = (float)PID_flywheel_q2_Buf / 1000000;
//    PID_flywheel.q3 = (float)PID_flywheel_q3_Buf / 10000;

//    ring.straight_max = (uint16)ring_straight_Buf*1000;
//    ring.close = (uint8)ring_close_Buf;
//    Ring_En = (uint8)ring_en_Buf;
//
//    garage.en_out = (int8)garage_en_out_Buf;
//    garage.en_in = (int8)garage_en_in_Buf;
//
//    if(forkroad.left_right == 0)
//    {
//        forkroad.left_en = 1;
//        forkroad.right_en = 0;
//    }
//    else if(forkroad.left_right == 1)
//    {
//        forkroad.left_en = 0;
//        forkroad.right_en = 1;
//    }
    /*****flashд��洢����*****/
//    flash_erase_page(0,EXAMPLE_EEPROM_SECTOR);     //����0����
//    flash_erase_page(0,VOFA_EEPROM_SECTOR);     //����0����

    flash_erase_page(0,ZL_EEPROM_SECTOR);     //����0����
    system_delay_ms(10);
    UDF_flash_page_program(ZL_EEPROM_SECTOR, (uint32*) &PID_ZL_jiaosudu_Kp_Buf, PID_ZL_jiaosudu_Kp_ADDR);
    UDF_flash_page_program(ZL_EEPROM_SECTOR, (uint32*) &PID_ZL_jiaosudu_Ki_Buf, PID_ZL_jiaosudu_Ki_ADDR);
    system_delay_ms(10);
    UDF_flash_page_program(ZL_EEPROM_SECTOR, (uint32*) &PID_ZL_jiaodu_Kp_Buf, PID_ZL_jiaodu_Kp_ADDR);
    UDF_flash_page_program(ZL_EEPROM_SECTOR, (uint32*) &PID_ZL_jiaodu_Kd_Buf, PID_ZL_jiaodu_Kd_ADDR);
    system_delay_ms(10);
    UDF_flash_page_program(ZL_EEPROM_SECTOR, (uint32*) &PID_ZL_Speed_Kp_Buf, PID_ZL_Speed_Kp_ADDR);
    UDF_flash_page_program(ZL_EEPROM_SECTOR, (uint32*) &PID_ZL_Speed_Ki_Buf, PID_ZL_Speed_Ki_ADDR);
    UDF_flash_page_program(ZL_EEPROM_SECTOR, (uint32*) &PID_ZL_Speed_Kd_Buf, PID_ZL_Speed_Kd_ADDR);
    system_delay_us(10);
    UDF_flash_page_program(ZL_EEPROM_SECTOR, (uint32*) &TURN_Diff_P_Buf, TURN_Diff_P_ADDR);
    UDF_flash_page_program(ZL_EEPROM_SECTOR, (uint32*) &TURN_Diff_D_Buf, TURN_Diff_D_ADDR);
    system_delay_ms(10);
    UDF_flash_page_program(ZL_EEPROM_SECTOR, (uint32*) &AngleAim_Buf, AngleAim_Buf_ADDR);
    UDF_flash_page_program(ZL_EEPROM_SECTOR, (uint32*) &Speed_Aim_Buf, Speed_Aim_Buf_ADDR);
    system_delay_ms(10);
    UDF_flash_page_program(ZL_EEPROM_SECTOR, (uint32*) &Exp_Time_Buf, Exp_Time_Buf_ADDR);
    UDF_flash_page_program(ZL_EEPROM_SECTOR, (uint32*) &Gain_Buf, Gain_Buf_ADDR);
    /*****flashд��洢����*****/
    menu_en = 0;
    oled_clear();
    system_delay_us(10);
}

/*******************UDF*******************/
/*         **��дFlash��д����**         */
/*         **  author:�ֹ���  **         */
/*         ** time:2021.11.23 **         */
/*******************UDF*******************/
void UDF_flash_page_read (uint32 sector_num, uint32 *buf, uint32 addr)
{
    *buf = *(uint32 *)((uint32)((EEPROM_BASE_ADDR + (sector_num)*EEPROM_PAGE_SIZE) + (addr*8)));
}
uint8 UDF_flash_page_program (uint32 sector_num, const uint32 *buf, uint32 addr)
{
    zf_assert(EEPROM_PAGE_NUM>sector_num);
    zf_assert(EEPROM_PAGE_LENGTH>addr);
    uint32 errors = 0;
    uint32 flash = 0;
    uint16 end_init_sfty_pw;
    uint32 sector_addr = IfxFlash_dFlashTableEepLog[sector_num].start;

    end_init_sfty_pw = IfxScuWdt_getSafetyWatchdogPassword();

    uint32 page_addr = sector_addr + addr * IFXFLASH_DFLASH_PAGE_LENGTH;
    errors = IfxFlash_enterPageMode(page_addr);
    zf_assert(0 == errors);

    IfxFlash_waitUnbusy(flash, IfxFlash_FlashType_D0);

    IfxFlash_loadPage(page_addr, buf[0], 0);

    IfxScuWdt_clearSafetyEndinit(end_init_sfty_pw);
    IfxFlash_writePage(page_addr);
    IfxScuWdt_setSafetyEndinit(end_init_sfty_pw);

    IfxFlash_waitUnbusy(flash, IfxFlash_FlashType_D0);
    return 0;
}
//////*******************UDF********************////

//void UDF_flash_write(void)
//{
//    Speed_Aim_Buf = Speed_Aim;
////    zero_p_r_Buf = zero_p_r;
////    zero_p_l_Buf = zero_p_l;
//
//    system_delay_us(10);
//    eeprom_erase_sector(VOFA_EEPROM_SECTOR);     //����0����
//    UDF_flash_page_program(VOFA_EEPROM_SECTOR, (uint32*) &Speed_Aim_Buf, SPEED_AIM_ADDR);
////    UDF_flash_page_program(VOFA_EEPROM_SECTOR, (uint32*) &zero_p_r, zero_p_r_ADDR);
////    UDF_flash_page_program(VOFA_EEPROM_SECTOR, (uint32*) &zero_p_l, zero_p_l_ADDR);
//
////    UDF_flash_page_program(VOFA_EEPROM_SECTOR, (uint32*) &PID_turn_Servo.q1, PID_turn_speed_q1_ADDR);
////    UDF_flash_page_program(VOFA_EEPROM_SECTOR, (uint32*) &PID_turn_Servo.q2, PID_turn_speed_q2_ADDR);
////    UDF_flash_page_program(VOFA_EEPROM_SECTOR, (uint32*) &PID_turn_Servo.q3, PID_turn_speed_q3_ADDR);
//    printf("write OK!\r\n");
//}
